#include <buola/cv/ccloud.h>
#include <buola/scene.h>
#include <buola/scene/geodes/cpointcloud.h>
#include <buola/app/ceventloop.h>
#include <buola/image/io.h>
#include <buola/image/cgc.h>
#include <buola/image/algorithm/filter.h>
#include <buola/app/ccmdline.h>
#include <buola/app/capp.h>
#include <buola/scene/cscene.h>
#include <buola/scene/cimagerenderer.h>
#include <buola/scene/csceneview.h>
#include <buola/scene/cperspectivecamera.h>
#include <buola/scene/transform/crttransform.h>
#include <buola/gui/ctoolkit.h>
#include <buola/scene/geodes/ccube.h>
#include <buola/scene/controllers/ccameracontroller.h>

using namespace buola;

int main(int pNArg,char **pArgs)
{
    buola_init(pNArg,pArgs);

    if(cmd_line().ArgCount()<1)
    {
        msg_info() << "please specify filename\n";
        return 1;
    }

//     mat::CVec3d lCameraAt{0,0,0};
//     mat::CVec3d lCameraPos{1,0,1};
//     mat::CVec3d lCameraUp{0,0,1};
    mat::CVec3d lCameraAt{0,0,1};
    mat::CVec3d lCameraPos{0,0,0};
    mat::CVec3d lCameraUp{0,-1,0};
    
    try
    {
        CApp lApp;
        
        gui::get_toolkit()->GetOpenGLGC()->MakeCurrent();

        //create the scene
        scene::PScene lScene=new scene::CScene;
        
        scene::PCube lCube=new scene::CCube;
        scene::PRTTransform lTransform=new scene::CRTTransform(mat::CQuat_d(),{0,0,5});
        lCube->AttachTo(lTransform);
        lScene->World()->AddChild(lTransform);

        scene::PPointCloud lCloud=new scene::CPointCloud();
        lScene->World()->AddObject(lCloud);

        scene::PPerspectiveCamera lCamera=new scene::CPerspectiveCamera;
        lCamera->SetFOV(2*atan2(240.0,525.0));
        lCamera->SetClipping(0.1,50.0);
        lCamera->LookAt(lCameraAt,lCameraPos,lCameraUp);

        scene::PSceneView lView=new scene::CSceneView;
        lView->SetCamera(lCamera);
        lView->AddScene(lScene);
        lView->AddController(new scene::CCameraController(lCamera));
        lView->Map(CSize_d(800,600));

#if 0        
        img::CImage_rgb8 lRGB;
        img::CImage_gray32f lDepth;

        if(cmd_line().ArgCount()==1)
        {
            img::load_rgbd(lRGB,lDepth,io::arg(0));
        }
        else
        {
            img::load(lRGB,io::arg(0));
            img::load_ascii(lDepth,io::arg(1),lRGB.Size());
        }

        ///////start test
        {
            img::CCameraParameters lParams;
            lParams.SetMatrix(CPoint_d(525,525),CPoint_d(320,240));
            start_timer();
            img::CImage_xyz32f lPosImage=cvi::make_pos_image(lDepth,lParams);
            img::CImage_rgb8 lRGBImage=clone(lRGB);
            end_timer();
            start_timer();
            for(int y=0;y<480;y++)
            {
                auto lPosIt=lPosImage.Row(y).begin();
                auto lRGBIt=lRGBImage.Row(y).begin();
                for(int x=0;x<640;x++)
                {
                    if(lPosIt[x][0]<0.0f)
                    {
                        lRGBIt[x][0]=0;
                    }                        
                }
            }
            end_timer();
            start_timer();
            auto lCloud=cvi::make_cloud(lPosImage,lRGBImage);
            end_timer();
            cvi::save_cloud(lCloud,io::file("a.pcd"));
        }
        {
            img::CCameraParameters lParams;
            lParams.SetMatrix(CPoint_d(525,525),CPoint_d(320,240));
            start_timer();
            cvi::CImage_point_rgb8 lPointImage=cvi::make_point_image(lDepth,lRGB,lParams);
            end_timer();
            start_timer();
            for(int y=0;y<480;y++)
            {
                auto lRow=lPointImage.Row(y);
                for(auto lPointIt=lRow.begin();lPointIt!=lRow.end();++lPointIt)
                {
                    if((*lPointIt)->mPoint.x()<0.0f)
                    {
                        (*lPointIt)->mPixel[0]=0;
                    }                        
                }
            }
            end_timer();
            start_timer();
            auto lCloud=cvi::CCloud_rgb8(lPointImage);
            end_timer();
            cvi::save_cloud(lCloud,io::file("b.pcd"));
        }
        {
            img::CCameraParameters lParams;
            lParams.SetMatrix(CPoint_d(525,525),CPoint_d(320,240));
            start_timer();
            auto lRegularCloud=cvi::make_cloud(lDepth,lRGB,lParams);
            end_timer();
            start_timer();
            auto lNormalCloud=cvi::make_cloud_with_normals(lDepth,lRGB,lParams,3);
            end_timer();
            msg_info() << lRegularCloud.size() << " points\n";
            msg_info() << lNormalCloud.size() << " normal points\n";
        }
        ///////end test
        
        
        
        img::CImage_gray32f lFilteredDepth(lDepth.Size());
        msg_info() << "filtering!!\n";
        start_timer();
        img::bilateral_filter(lDepth,lFilteredDepth,9,0.05);
        end_timer();
        img::CCameraParameters lParams;
        lParams.SetMatrix(CPoint_d(525,525),CPoint_d(320,240));
        lCloud->SetCloud(cvi::make_cloud_with_normals(lFilteredDepth,lRGB,lParams,3));
#endif
        lCloud->SetCloud(cvi::load_cloud(io::arg(0)));

        lApp.Run();
    }
    catch(std::exception &pE)
    {
        msg_info() << "caught exception " << pE.what() << "\n";
    }

    return buola_finish();
}
